#include "ANO_DT.h"
#include "dataconvert.h"

typedef struct //是否发送标志位
{
    uint8_t send_pid1;
    uint8_t send_pid2;
    uint8_t send_pid3;
    uint8_t send_pid4;
    uint8_t send_pid5;
    uint8_t send_pid6;
} dt_flag_t;

typedef struct //PID
{
    u16 kp;
    u16 ki;
    u16 kd;
} dt_pid_t;

typedef struct //数据存放区
{
    dt_pid_t PID[18];
} dt_ctrl_t;

// 通信UART口定义
#if (ANO_DT_UART_PORT_SELECT == 1)
#include "usart.h"
#define ANO_DT_UART_PORT USART1
#elif (ANO_DT_UART_PORT_SELECT == 2)
#include "usart2.h"
#define ANO_DT_UART_PORT USART2
#elif (ANO_DT_UART_PORT_SELECT == 3)
#include "usart3.h"
#define ANO_DT_UART_PORT USART3
#elif (ANO_DT_UART_PORT_SELECT == 4)
#include "usart4.h"
#define ANO_DT_UART_PORT USART4
#endif

//数据拆分宏定义，在发送大于1字节的数据类型时，比如int16、float等
//需要把数据拆分成单独字节进行发送
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp) + 0) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp) + 1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp) + 2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp) + 3) )

// 参数缩放比例的倒数
#define ANO_DT_PARAMETER_SCALINT_RECIPROCAL (1/ANO_DT_PARAMETER_SCALING)
#define ANO_DT_CTRL_TYPE      (uint16_t)   //发送到上位机的参数类型

/* Private variables ---------------------------------------------------------*/

enum pid { //数据数组的序号。
    PID1 = 0,
    PID2,
    PID3,
    PID4,
    PID5,
    PID6,
    PID7,
    PID8,
    PID9,
    PID10,
    PID11,
    PID12,
    PID13,
    PID14,
    PID15,
    PID16,
    PID17,
    PID18
};

static dt_flag_t f;				//需要发送数据的标志
static dt_ctrl_t ctrl_1;       	//存放数据--中转
static uint8_t data_to_send[50];	//发送数据缓存
static uint8_t ANO_DT_RxBuffer[50]; //接收数据缓存
static uint8_t ANO_DT_Rx_Cnt = 0;   //接收数据计数


/* Private function prototypes -----------------------------------------------*/

static void ANO_DT_Data_Exchange(void);
static uint8_t ANO_DT_Data_Receive_Prepare(uint8_t data);
static void ANO_DT_Data_Receive_Anl(uint8_t *data_buf, uint8_t num);
static void ANO_DT_Send_Data(uint8_t *dataToSend, uint8_t length);
static void ANO_DT_Send_PID(uint8_t group,
                            float p1_p, float p1_i, float p1_d,
                            float p2_p, float p2_i, float p2_d,
                            float p3_p, float p3_i, float p3_d);
static void ANO_DT_Send_Check(uint8_t head, uint8_t check_sum);
static void Argument_Save(void);
static void Ctrl_To_Argument(void);
static void Ctrl_Set_Zoro(void);
static void Flash_ReadPID(void);
static void Flash_SavePID(void);
void Argument_Load(void);
void U_putchar(u8 ch);

// 接受中断函数
#if (ANO_DT_UART_PORT_SELECT == 1)
void USART1_IRQHandler(void)
#elif (ANO_DT_UART_PORT_SELECT == 2)
void USART2_IRQHandler(void)
#elif (ANO_DT_UART_PORT_SELECT == 3)
void USART3_IRQHandler(void)
#elif (ANO_DT_UART_PORT_SELECT == 4)
void USART4_IRQHandler(void)
#endif
{
    if (USART_GetFlagStatus(ANO_DT_UART_PORT, USART_FLAG_RXNE) != RESET)  //接收到数据
    {
        uint16_t res = 0;
        res = USART_ReceiveData(ANO_DT_UART_PORT);

        ANO_DT_Data_Receive_Prepare((uint8_t)res);
    }
}

//系统初始化化时调用这个函数，从FLASH中读出数赋值给中间变量，进而对用户变量进行赋值
void Argument_Init(void)
{
    Ctrl_Set_Zoro();
    Flash_ReadPID();
    Ctrl_To_Argument();
}

//接受数据准备，并判断是读取还是写入数据
uint8_t state = 0;
uint8_t ANO_DT_Data_Receive_Prepare(uint8_t data)
{
    static uint8_t _data_len = 0;
    //static uint8_t state = 0;

    if (state == 0 && data == 0xAA)
    {
        state = 1;
        ANO_DT_RxBuffer[0] = data;
    }
    else if (state == 1 && data == 0xAF)
    {
        state = 2;
        ANO_DT_RxBuffer[1] = data;
    }
    else if (state == 2 && data<0XF1)
    {
        state = 3;
        ANO_DT_RxBuffer[2] = data;
    }
    else if (state == 3 && data<50)
    {
        state = 4;
        ANO_DT_RxBuffer[3] = data;
        _data_len = data;
        ANO_DT_Rx_Cnt = 0;
    }
    else if (state == 4 && _data_len>0)
    {
        _data_len--;
        ANO_DT_RxBuffer[4 + ANO_DT_Rx_Cnt++] = data;
        if (_data_len == 0)
            state = 5;
    }
    else if (state == 5)
    {
        state = 0;
        ANO_DT_RxBuffer[4 + ANO_DT_Rx_Cnt] = data;
        ANO_DT_Data_Receive_Anl(ANO_DT_RxBuffer, ANO_DT_Rx_Cnt);
        return Ready;
    }
    else
        state = 0;

    return Not_Ready;
}

static void ANO_DT_Data_Receive_Anl(uint8_t *data_buf, uint8_t num)
{
    num = num + 5;
    uint8_t sum = 0;
    for (uint8_t i = 0; i<(num - 1); i++)
        sum += *(data_buf + i);
    if (!(sum == *(data_buf + num - 1)))		return;		//判断sum
    if (!(*(data_buf) == 0xAA && *(data_buf + 1) == 0xAF))		return;		//判断帧头

    //向上位机发送当前数据
    if (*(data_buf + 2) == 0X02)//上位机读取数据，重要！！！
    {
        if (*(data_buf + 4) == 0X01)//读取六组PID的值
        {
            Argument_Load();//加载flash的值，并且分配
            f.send_pid1 = 1;//18组PID 分成 6次 发送
            f.send_pid2 = 1;
            f.send_pid3 = 1;
            f.send_pid4 = 1;
            f.send_pid5 = 1;
            f.send_pid6 = 1;
            ANO_DT_Data_Exchange();	//发送数据到上位机
        }
    }

    //写入修改 1~3 组PID
    if (*(data_buf + 2) == 0X10)
    {
        ctrl_1.PID[PID1].kp = ((vu16)(*(data_buf + 4) << 8) | *(data_buf + 5));
        ctrl_1.PID[PID1].ki = ((vu16)(*(data_buf + 6) << 8) | *(data_buf + 7));
        ctrl_1.PID[PID1].kd = ((vu16)(*(data_buf + 8) << 8) | *(data_buf + 9));
        ctrl_1.PID[PID2].kp = ((vu16)(*(data_buf + 10) << 8) | *(data_buf + 11));
        ctrl_1.PID[PID2].ki = ((vu16)(*(data_buf + 12) << 8) | *(data_buf + 13));
        ctrl_1.PID[PID2].kd = ((vu16)(*(data_buf + 14) << 8) | *(data_buf + 15));
        ctrl_1.PID[PID3].kp = ((vu16)(*(data_buf + 16) << 8) | *(data_buf + 17));
        ctrl_1.PID[PID3].ki = ((vu16)(*(data_buf + 18) << 8) | *(data_buf + 19));
        ctrl_1.PID[PID3].kd = ((vu16)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    //写入修改 4~6 组PID
    if (*(data_buf + 2) == 0X11)
    {
        ctrl_1.PID[PID4].kp = ((vu16)(*(data_buf + 4) << 8) | *(data_buf + 5));
        ctrl_1.PID[PID4].ki = ((vu16)(*(data_buf + 6) << 8) | *(data_buf + 7));
        ctrl_1.PID[PID4].kd = ((vu16)(*(data_buf + 8) << 8) | *(data_buf + 9));
        ctrl_1.PID[PID5].kp = ((vu16)(*(data_buf + 10) << 8) | *(data_buf + 11));
        ctrl_1.PID[PID5].ki = ((vu16)(*(data_buf + 12) << 8) | *(data_buf + 13));
        ctrl_1.PID[PID5].kd = ((vu16)(*(data_buf + 14) << 8) | *(data_buf + 15));
        ctrl_1.PID[PID6].kp = ((vu16)(*(data_buf + 16) << 8) | *(data_buf + 17));
        ctrl_1.PID[PID6].ki = ((vu16)(*(data_buf + 18) << 8) | *(data_buf + 19));
        ctrl_1.PID[PID6].kd = ((vu16)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    //写入修改 7~9 组PID
    if (*(data_buf + 2) == 0X12)
    {
        ctrl_1.PID[PID7].kp = ((vu16)(*(data_buf + 4) << 8) | *(data_buf + 5));
        ctrl_1.PID[PID7].ki = ((vu16)(*(data_buf + 6) << 8) | *(data_buf + 7));
        ctrl_1.PID[PID7].kd = ((vu16)(*(data_buf + 8) << 8) | *(data_buf + 9));
        ctrl_1.PID[PID8].kp = ((vu16)(*(data_buf + 10) << 8) | *(data_buf + 11));
        ctrl_1.PID[PID8].ki = ((vu16)(*(data_buf + 12) << 8) | *(data_buf + 13));
        ctrl_1.PID[PID8].kd = ((vu16)(*(data_buf + 14) << 8) | *(data_buf + 15));
        ctrl_1.PID[PID9].kp = ((vu16)(*(data_buf + 16) << 8) | *(data_buf + 17));
        ctrl_1.PID[PID9].ki = ((vu16)(*(data_buf + 18) << 8) | *(data_buf + 19));
        ctrl_1.PID[PID9].kd = ((vu16)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    //写入修改 10~12 组PID
    if (*(data_buf + 2) == 0X13)
    {
        ctrl_1.PID[PID10].kp = ((vu16)(*(data_buf + 4) << 8) | *(data_buf + 5));
        ctrl_1.PID[PID10].ki = ((vu16)(*(data_buf + 6) << 8) | *(data_buf + 7));
        ctrl_1.PID[PID10].kd = ((vu16)(*(data_buf + 8) << 8) | *(data_buf + 9));
        ctrl_1.PID[PID11].kp = ((vu16)(*(data_buf + 10) << 8) | *(data_buf + 11));
        ctrl_1.PID[PID11].ki = ((vu16)(*(data_buf + 12) << 8) | *(data_buf + 13));
        ctrl_1.PID[PID11].kd = ((vu16)(*(data_buf + 14) << 8) | *(data_buf + 15));
        ctrl_1.PID[PID12].kp = ((vu16)(*(data_buf + 16) << 8) | *(data_buf + 17));
        ctrl_1.PID[PID12].ki = ((vu16)(*(data_buf + 18) << 8) | *(data_buf + 19));
        ctrl_1.PID[PID12].kd = ((vu16)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    //写入修改 13~15 组PID
    if (*(data_buf + 2) == 0X14)
    {
        ctrl_1.PID[PID13].kp = ((vu16)(*(data_buf + 4) << 8) | *(data_buf + 5));
        ctrl_1.PID[PID13].ki = ((vu16)(*(data_buf + 6) << 8) | *(data_buf + 7));
        ctrl_1.PID[PID13].kd = ((vu16)(*(data_buf + 8) << 8) | *(data_buf + 9));
        ctrl_1.PID[PID14].kp = ((vu16)(*(data_buf + 10) << 8) | *(data_buf + 11));
        ctrl_1.PID[PID14].ki = ((vu16)(*(data_buf + 12) << 8) | *(data_buf + 13));
        ctrl_1.PID[PID14].kd = ((vu16)(*(data_buf + 14) << 8) | *(data_buf + 15));
        ctrl_1.PID[PID15].kp = ((vu16)(*(data_buf + 16) << 8) | *(data_buf + 17));
        ctrl_1.PID[PID15].ki = ((vu16)(*(data_buf + 18) << 8) | *(data_buf + 19));
        ctrl_1.PID[PID15].kd = ((vu16)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    //写入修改 16~18 组PID
    if (*(data_buf + 2) == 0X15)
    {
        ctrl_1.PID[PID16].kp = ((vu16)(*(data_buf + 4) << 8) | *(data_buf + 5));
        ctrl_1.PID[PID16].ki = ((vu16)(*(data_buf + 6) << 8) | *(data_buf + 7));
        ctrl_1.PID[PID16].kd = ((vu16)(*(data_buf + 8) << 8) | *(data_buf + 9));
        ctrl_1.PID[PID17].kp = ((vu16)(*(data_buf + 10) << 8) | *(data_buf + 11));
        ctrl_1.PID[PID17].ki = ((vu16)(*(data_buf + 12) << 8) | *(data_buf + 13));
        ctrl_1.PID[PID17].kd = ((vu16)(*(data_buf + 14) << 8) | *(data_buf + 15));
        ctrl_1.PID[PID18].kp = ((vu16)(*(data_buf + 16) << 8) | *(data_buf + 17));
        ctrl_1.PID[PID18].ki = ((vu16)(*(data_buf + 18) << 8) | *(data_buf + 19));
        ctrl_1.PID[PID18].kd = ((vu16)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
        Argument_Save();
		Posotion_SetANO();
    }
}

// 向上位机发送中间变量参数
static void ANO_DT_Send_PID(uint8_t group,
                            float p1_p, float p1_i, float p1_d,
                            float p2_p, float p2_i, float p2_d,
                            float p3_p, float p3_i, float p3_d)
{
    uint8_t _cnt = 0;
    vs16 _temp;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x10 + group - 1;
    data_to_send[_cnt++] = 0;


    _temp = p1_p;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p1_i;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p1_d;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p2_p;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p2_i;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p2_d;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p3_p;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p3_i;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p3_d;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i<_cnt; i++)
        sum += data_to_send[i];

    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}

//通过串口发送一定长度的数据到上位机
static void ANO_DT_Send_Data(uint8_t *dataToSend, uint8_t length)
{
    while (length--)
    {
        U_putchar(*dataToSend);
        dataToSend++;
    }
}

// 发送一个字节
void U_putchar(u8 ch)
{
#if (ANO_DT_UART_PORT_SELECT == 1)
    while ((USART1->SR & 0X40) == 0);//串口1循环发送,直到发送完毕
    USART1->DR = (u8)ch;
#elif (ANO_DT_UART_PORT_SELECT == 2)
    while ((USART2->SR & 0X40) == 0);//串口2循环发送,直到发送完毕
    USART2->DR = (u8)ch;
#elif (ANO_DT_UART_PORT_SELECT == 3)
    while ((USART3->SR & 0X40) == 0);//串口3循环发送,直到发送完毕
    USART3->DR = (u8)ch;
#elif (ANO_DT_UART_PORT_SELECT == 4)
    while ((USART4->SR & 0X40) == 0);//串口4循环发送,直到发送完毕
    USART4->DR = (u8)ch;
#endif
}

//储存中间变量数据到FLASH里
static void Flash_SavePID(void)
{
    u8 var;

    u32 pid_flash_buf[54];
    memset(pid_flash_buf, 0, sizeof(pid_flash_buf));

    //数据缓存到pid_flash_buf中
    for (var = 0; var < 18; var++)
    {
        *(pid_flash_buf + 3 * var + 0) = ctrl_1.PID[var].kp;
        *(pid_flash_buf + 3 * var + 1) = ctrl_1.PID[var].ki;
        *(pid_flash_buf + 3 * var + 2) = ctrl_1.PID[var].kd;
    }

    //写入flash操作,擦除数据已经包含在函数内
    STMFLASH_Write(ANO_DT_PID_DATA_SAVE_FLASH_ADDRESS, pid_flash_buf, 54);
}

//发送数据到单片机
static void ANO_DT_Data_Exchange(void)
{

    if (f.send_pid1)
    {
        f.send_pid1 = 0;
        ANO_DT_Send_PID(1,
                        ctrl_1.PID[PID1].kp, ctrl_1.PID[PID1].ki, ctrl_1.PID[PID1].kd,

                        ctrl_1.PID[PID2].kp, ctrl_1.PID[PID2].ki, ctrl_1.PID[PID2].kd,

                        ctrl_1.PID[PID3].kp, ctrl_1.PID[PID3].ki, ctrl_1.PID[PID3].kd);
    }

    if (f.send_pid2)
    {
        f.send_pid2 = 0;
        ANO_DT_Send_PID(2,
                        ctrl_1.PID[PID4].kp, ctrl_1.PID[PID4].ki, ctrl_1.PID[PID4].kd,

                        ctrl_1.PID[PID5].kp, ctrl_1.PID[PID5].ki, ctrl_1.PID[PID5].kd,

                        ctrl_1.PID[PID6].kp, ctrl_1.PID[PID6].ki, ctrl_1.PID[PID6].kd);
    }

    if (f.send_pid3)
    {
        f.send_pid3 = 0;
        ANO_DT_Send_PID(3,
                        ctrl_1.PID[PID7].kp, ctrl_1.PID[PID7].ki, ctrl_1.PID[PID7].kd,

                        ctrl_1.PID[PID8].kp, ctrl_1.PID[PID8].ki, ctrl_1.PID[PID8].kd,

                        ctrl_1.PID[PID9].kp, ctrl_1.PID[PID9].ki, ctrl_1.PID[PID9].kd);
    }

    if (f.send_pid4)
    {
        f.send_pid4 = 0;
        ANO_DT_Send_PID(4,
                        ctrl_1.PID[PID10].kp, ctrl_1.PID[PID10].ki, ctrl_1.PID[PID10].kd,

                        ctrl_1.PID[PID11].kp, ctrl_1.PID[PID11].ki, ctrl_1.PID[PID11].kd,

                        ctrl_1.PID[PID12].kp, ctrl_1.PID[PID12].ki, ctrl_1.PID[PID12].kd);
    }

    if (f.send_pid5)
    {
        f.send_pid5 = 0;
        ANO_DT_Send_PID(5,
                        ctrl_1.PID[PID13].kp, ctrl_1.PID[PID13].ki, ctrl_1.PID[PID13].kd,

                        ctrl_1.PID[PID14].kp, ctrl_1.PID[PID14].ki, ctrl_1.PID[PID14].kd,

                        ctrl_1.PID[PID15].kp, ctrl_1.PID[PID15].ki, ctrl_1.PID[PID15].kd);
    }

    if (f.send_pid6)
    {
        f.send_pid6 = 0;
        ANO_DT_Send_PID(6,
                        ctrl_1.PID[PID16].kp, ctrl_1.PID[PID16].ki, ctrl_1.PID[PID16].kd,

                        ctrl_1.PID[PID17].kp, ctrl_1.PID[PID17].ki, ctrl_1.PID[PID17].kd,

                        ctrl_1.PID[PID18].kp, ctrl_1.PID[PID18].ki, ctrl_1.PID[PID18].kd);
    }

}

//数据帧头校验
static void ANO_DT_Send_Check(uint8_t head, uint8_t check_sum)
{
    data_to_send[0] = 0xAA;
    data_to_send[1] = 0xAA;
    data_to_send[2] = 0xEF;
    data_to_send[3] = 2;
    data_to_send[4] = head;
    data_to_send[5] = check_sum;


    uint8_t sum = 0;
    for (uint8_t i = 0; i<6; i++)
        sum += data_to_send[i];
    data_to_send[6] = sum;

    ANO_DT_Send_Data(data_to_send, 7);
}

//用于单片机终端接受上位机发来的数据的校验和功能的分配


//从FLASH里读取PID数据
static void Flash_ReadPID(void)
{
    u8 var;

    u32 pid_flash_buf[54];
    memset(pid_flash_buf, 0, sizeof(pid_flash_buf));

    STMFLASH_Read(ANO_DT_PID_DATA_SAVE_FLASH_ADDRESS, pid_flash_buf, 54);

    //从缓存中读取数据
    for (var = 0; var < 18; var++)
    {
        ctrl_1.PID[var].kp = (u16)*(pid_flash_buf + 3 * var + 0);
        ctrl_1.PID[var].ki = (u16)*(pid_flash_buf + 3 * var + 1);
        ctrl_1.PID[var].kd = (u16)*(pid_flash_buf + 3 * var + 2);
    }
}

//读取FLASH的值到中间变量
void Argument_Load(void)
{
    Ctrl_Set_Zoro();
    Flash_ReadPID();
}

//从上位机得到的中间变量的值保存到FLASH,然后赋值给自己定义的变量
static void Argument_Save(void)
{
    Flash_SavePID();
    Ctrl_To_Argument();
}

// 设置Ctrl_1的数据清0
static void Ctrl_Set_Zoro(void)
{
    memset(&ctrl_1, 0, sizeof(dt_ctrl_t));
}

//中间变量的值给自己定义的变量
static void Ctrl_To_Argument(void)
{
#if defined(USE_PID_01)
    //PID1
    PID_01_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID1].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_01_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID1].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_01_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID1].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_02)
    //PID2
    PID_02_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID2].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_02_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID2].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_02_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID2].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_03)
    //PID3
    PID_03_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID3].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_03_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID3].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_03_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID3].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_04)
    //PID4
    PID_04_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID4].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_04_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID4].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_04_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID4].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_05)
    //PID5
    PID_05_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID5].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_05_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID5].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_05_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID5].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_06)
    //PID6
    PID_06_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID6].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_06_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID6].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_06_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID6].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_07)
    //PID7
    PID_07_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID7].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_07_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID7].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_07_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID7].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_08)
    //PID8
    PID_08_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID8].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_08_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID8].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_08_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID8].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_09)
    //PID9
    PID_09_P = -ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID9].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_09_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID9].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_09_D = -ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID9].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_10)
    //PID10
    PID_10_P = -ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID10].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_10_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID10].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_10_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID10].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_11)
    //PID11
    PID_11_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID11].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_11_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID11].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_11_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID11].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_12)
    //PID12
    PID_12_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID12].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_12_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID12].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_12_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID12].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_13)
    //PID13
    PID_13_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID13].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_13_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID13].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_13_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID13].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_14)
    //PID14
    PID_14_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID14].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_14_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID14].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_14_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID14].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_15)
    //PID15
    PID_15_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID15].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_15_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID15].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_15_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID15].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_16)
    //PID16
    PID_16_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID16].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_16_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID16].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_16_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID16].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_17)
    //PID17
    PID_17_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID17].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_17_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID17].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_17_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID17].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif

#if defined(USE_PID_18)
    //PID18
    PID_18_P = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID18].kp * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_18_I = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID18].ki * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
    PID_18_D = ANO_DT_PARAMETER_TYPE((double)ctrl_1.PID[PID18].kd * ANO_DT_PARAMETER_SCALINT_RECIPROCAL);
#endif
}

/******************* (C) COPYRIGHT 2016 ANO TECH *******END OF FILE************/
